#define _CRT_SECURE_NO_WARNINGS
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <iostream>
#include <cstdio>
#include <vector>
#include <time.h>
#include "Mypoint.h"

using namespace std;
using namespace cv;

#define Red
#define videocannel (1)
#define videopath ("D:\\Sources\\moveCar1.avi")

const Mat camera_parameter = (Mat_<double>(3, 3) << 780.5095,        0, 341.6064,
														   0, 780.0744, 219.1356,
														   0,        0,   1.0000);
VideoCapture video;

void videolink() { video.open(videocannel); if (!video.isOpened())exit(114514); }
int frameproc(Mat &frame);
void videoproc();
vector<Point2f> getimagepoints(vector<RotatedRect> bars);

int main()
{
	videolink();
	videoproc();
	return 0;
}

vector<Point2f> getimagepoints(vector<RotatedRect> bars)
{
	Point2f p0, p1, p2, p3;
	vector<Point2f> ret;
	if (bars.size() < 2) { cout << "bad bars" << endl; return ret; }                           //  dbug
	RRPoint lb(bars[0], MP_XY_SUMS), rb(bars[1], MP_XY_SUMS);
	if (bars[0].center.x > bars[1].center.x + (1e-6)) swap(lb, rb), swap(bars[0], bars[1]);

	
	p0 = lb.p0; ret.push_back(p0);
	p1 = rb.p1; ret.push_back(p1);
	p2 = rb.p2; ret.push_back(p2);
	p3 = lb.p3; ret.push_back(p3);
	return ret;
}

int frameproc(Mat &frame)
{
	time_t start = clock();

	//-------------------------process pic---------------------------
	vector<RotatedRect> rects;
	vector<vector<Point>> contours;
	Mat channel[3]; split(frame, channel);
#ifdef Red
	Mat tar = -1.0 * channel[0] - 0 * channel[1] + 2.0 * channel[2]; 
#endif // Red
#ifdef Blue
	Mat tar = +2.0 * channel[0] - 0 * channel[1] - 1.0 * channel[2];
#endif // Blue
	threshold(tar, tar, 80, 255, THRESH_BINARY);

	Mat stru = getStructuringElement(MORPH_ELLIPSE, Size(5, 5), Point(2, 2));
	erode(tar, tar, stru);
	dilate(tar, tar, stru);
	findContours(tar, contours, RETR_TREE, CHAIN_APPROX_NONE);

#define FIT_ELLIPSE
	for (int i = 0; i < contours.size(); i++)
	{
#ifdef FIT_ELLIPSE
		if (contours[i].size() < 6) continue;
		RotatedRect rec = fitEllipse(contours[i]);
		if (rec.size.height < rec.size.width * 3) continue;
		if ((40 < rec.angle) && (rec.angle < 140)) continue;
#endif // FIT_ELLIPSE
#ifdef MIN_AREA
		if (contours[i].size() < 3) continue;
		RotatedRect rec = minAreaRect(contours[i]);
#endif // MIN_AREA
		rects.push_back(rec);
	}
#undef FIT_ELLIPSE


	//------------------------rect getpair-----------------------------
	if (rects.size() < 2) { cout << "more rect needed!" << endl; return 1; }
	vector<Point2f> imagepoints;
	RRPoint lb(rects[0], MP_XY_SUMS), rb(rects[1], MP_XY_SUMS);
	if (rects[0].center.x > rects[1].center.x + (1e-6)) swap(lb, rb);
	imagepoints.push_back(lb.p0);//p0
	imagepoints.push_back(rb.p1);//p1
	imagepoints.push_back(rb.p2);//p2
	imagepoints.push_back(lb.p3);//p3


	//-------------------------solvePnP-----------------------------
	vector<Point3f> objpoints;
	Mat rvec, tvec;
	objpoints.push_back(Point3f(58, 27.5, 0));
	objpoints.push_back(Point3f(128, 27, 0));
	objpoints.push_back(Point3f(128, -27, 0));
	objpoints.push_back(Point3f(58, -27.5, 0));
	if (objpoints.size() != imagepoints.size()) { cout << "points not match" << endl; return 2; }
	solvePnP(objpoints, imagepoints, camera_parameter, Mat(), rvec, tvec, false, CV_ITERATIVE);


	//-----------------------debug draw-------------------------
	Mat tar_con(tar.rows, tar.cols, CV_8UC3, Scalar(255, 255, 255));
	for (int i = 0; i < rects.size(); i++)
	{
		ellipse(tar_con, rects[i], Scalar(0, 0, 255));

	}
	putText(frame, "P0", imagepoints[0], FONT_HERSHEY_SIMPLEX, 0.45, Scalar(255, 255, 255));
	putText(frame, "P1", imagepoints[1], FONT_HERSHEY_SIMPLEX, 0.45, Scalar(255, 255, 255));
	putText(frame, "P2", imagepoints[2], FONT_HERSHEY_SIMPLEX, 0.45, Scalar(255, 255, 255));
	putText(frame, "P3", imagepoints[3], FONT_HERSHEY_SIMPLEX, 0.45, Scalar(255, 255, 255));
	circle(frame, imagepoints[0], 3, Scalar(0, 0, 255), 2);
	circle(frame, imagepoints[1], 3, Scalar(0, 255, 0), 2);
	circle(frame, imagepoints[2], 3, Scalar(255, 0, 0), 2);
	circle(frame, imagepoints[3], 3, Scalar(255, 0, 255), 2);
	imshow("tar", tar);
	imshow("tar_con", tar_con);
	imshow("frame", frame);
	printf("proc time = %lld   dist =", clock() - start,tvec.at<double>(2));
	for (int i = 0; i < tvec.at<double>(2); i+=4) putchar('*'); puts("");
	return 0; // successful processing
}



void videoproc()
{
	Mat nowframe;
	while (video.read(nowframe))
	{
		frameproc(nowframe);
		waitKey(33);
	}
}